#include "stm32f10x.h"

extern float Velocity_Kp, Velocity_Ki, Velocity_Kd;

/**
 * @brief Close loop control of the motor
 * @param TargetVelocity
 * @param CurrentVelocity
 * @retval pwm value of motor
*/
int32_t Velocity_A(int32_t TargetVelocity, int32_t CurrentVelocity)
{
    int32_t Bias;                                   // Define relevant variables
    static int32_t ControlVelocity, Last_bias;      // Static variable, whose value persists after the function call ends
    Bias = TargetVelocity - CurrentVelocity;        // Calculate speed deviation
    ControlVelocity += Velocity_Kp*(Bias - Last_bias) + Velocity_Ki * Bias;
                                                    // Incremental PID controller
                                                    // acts as a limit acceleration
                                                    // The speed control value is obtained by continuous integration of Bias,
                                                    // the greater the deviation, the greater the acceleration

    Last_bias = Bias;
    
    // if (ControlVelocity > 7199) ControlVelocity = 7199;
    // if (ControlVelocity < 0) ControlVelocity = 0;
    return ControlVelocity;                         // return speed control value
}

/**
 * @brief Close loop control of the motor
 * @param TargetVelocity
 * @param CurrentVelocity
 * @retval pwm value of motor
*/
int32_t Velocity_B(int32_t TargetVelocity, int32_t CurrentVelocity)
{
    int32_t Bias;                                   // Define relevant variables
    static int32_t ControlVelocity, Last_bias;      // Static variable, whose value persists after the function call ends
    Bias = TargetVelocity - CurrentVelocity;        // Calculate speed deviation
    ControlVelocity += Velocity_Kp*(Bias - Last_bias) + Velocity_Ki * Bias;
                                                    // Incremental PID controller
                                                    // acts as a limit acceleration
                                                    // The speed control value is obtained by continuous integration of Bias,
                                                    // the greater the deviation, the greater the acceleration

    Last_bias = Bias;
    
    // if (ControlVelocity > 7199) ControlVelocity = 7199;
    // if (ControlVelocity < 0) ControlVelocity = 0;
    return ControlVelocity;                         // return speed control value
}
